#include "world.h"

#include <gl\glfw.h>
#include <algorithm> // remove and remove_if

void World::startFrame()
{
	hardCollisions.clear();
	perfTimingList.clear();

	// Todo remove me
	for each(SpringBody *b in bodyList)
	{
		b->calcCenter();
		//assert( worldBounds.contains( &(b->bounds) ) );
	}
}

void World::update( double dt )
{
	double totalUpdate = glfwGetTime();

	for(size_t i=0; i<forceRegistry.size(); i++)
	{
		forceRegistry[i].fgen->update(forceRegistry[i].body, dt);
	}
	for(size_t i=0; i<bodyList.size(); i++)
	{
		bodyList[i]->isColliding = false;
	}

	double integrateStart = glfwGetTime();
	updateVerlet();
	integrateStart = glfwGetTime() - integrateStart;
	perfTimingList.push_back( PerfTiming("integratestart", integrateStart) );

	iterateCollisions();

	totalUpdate = glfwGetTime() - totalUpdate;
	perfTimingList.push_back( PerfTiming("totalUpdate", totalUpdate) );
}

void World::draw()
{
	glColor3f(1,1,0);
	this->worldBounds.draw();
	//myGrid->draw();

	for(size_t i=0; i<bodyList.size(); i++)
	{
		//if( bodyList[i]->inUse() )
			bodyList[i]->draw();
	}

	for(size_t i=0; i<particlesList.size(); i++)
	{
		Particle *p = particlesList[i];
		if( p->parentBody == 0x0 )
		{
			glBegin(GL_POINTS);
			glVertex3f( (float)p->pos.x(), (float)p->pos.y(), 0 );
			glEnd();
		}
	}
	
}

void World::debugDraw()
{
	glLineWidth(1.5);
	glEnable(GL_BLEND);
	glBlendFunc(GL_ONE, GL_ONE);
	glColor4f(0.1f, 0.1f, 0.7f, 0.5f);
	for(size_t i=0; i<bodyList.size(); i++)
	{
		bodyList[i]->bounds.draw();
	}
	glDisable(GL_BLEND);
}


void World::updateVerlet()
{
	for(size_t i=0; i<particlesList.size(); i++)
	{
		Particle *p = particlesList[i];
		//if( p->inUse() )
		{
			if( p->parentBody->isFrozen ) continue;

			Vec2f resultAccel = p->acceleration + p->forceAccum;

			Vec2f tmpPos = p->pos;
			p->pos += p->pos - p->oldPos + resultAccel * p->massInverse;
			p->oldPos = tmpPos;

			// Todo: better speed limit
			p->velocity = p->pos - p->oldPos;
			p->speed = length(p->velocity);
			if( p->speed > 1.0 )
			{
				p->oldPos += p->velocity * 0.5;
			}

			p->forceAccum.is(0.0, 0.0);
		}
	}  

}

void World::iterateCollisions()
{
	double timEdges = 0;
	double timColl=0;
	double timGridTest = 0;

	int phys_iterations = 10;
	for(int i=0; i<phys_iterations; i++)
	{
		double timEdges_delta = glfwGetTime();
		
		updateEdges(); 
		timEdges += glfwGetTime() - timEdges_delta;

		for(size_t j=0; j<bodyList.size(); j++)
		{
			bodyList[j]->calcCenter();
		}
		double timColl_delta = glfwGetTime();
		
		
		if(useBruteforce)
		{
			testCollision();
		}else
		{
			//myGrid->startFrame();
			double timGridTest_delta = glfwGetTime();

			myGrid->clearGrid();
			myGrid->scan();
			myGrid->doInteractions();
			timGridTest +=  glfwGetTime() - timGridTest_delta;

			const std::vector<CollisionPair> &cPairList = myGrid->collisionPairs;
			for(int j=0; j<myGrid->numCollisionPairs; j++)
			{
				 fineTest( cPairList[j].a, cPairList[j].b );
			}
			

			
			
		}
		timColl += glfwGetTime() - timColl_delta;
	}

	perfTimingList.push_back( PerfTiming("timEdges", timEdges) );
	perfTimingList.push_back( PerfTiming("timColl", timColl) );
	perfTimingList.push_back( PerfTiming("timGridTest", timGridTest) );
}

void World::registerForceGenerator( IForceGenerator * fgen, SpringBody * body )
{
	ForceRegistration freg;
	freg.body = body;
	freg.fgen = fgen;
	forceRegistry.push_back( freg );
}

void World::updateEdges()
{
	for(size_t i=0; i < edgeList.size(); i++)
	{
		Edge *e = edgeList[i];
		//if( e->inUse() )
		{
		Particle *p1 = e->particles[0];
		Particle *p2 = e->particles[1];

		Vec2f p1p2 = p2->pos - p1->pos;	
/*		real V1V2Length = length(p1p2);		
		real Diff       = V1V2Length - e->originalLength;				
		normalize(p1p2);	
		real InvEdgeWeight = p1->massInverse + p2->massInverse; 

		***New***		
		float Ratio1 = 1.0 / p2->massInverse*InvEdgeWeight;		
		float Ratio2 = 1.0 / p1->massInverse*InvEdgeWeight;		
		p1->pos += p1p2*Diff*Ratio1*0.5;		
		p2->pos -= p1p2*Diff*Ratio2*0.5;
*/	

		real dSquared = length_2(p1p2);

#if 0
		// Rod force, useful to reduce springiness... but constraints will pop tru others if moved too far during a frame
		real f = 0;
		real d = 0;
		if (dSquared <= 0){ 
			f = 0;
		}else{ 
			d = sqrt(dSquared); 
			f = (e->originalLength-d)/d; 
		}
#else      
		// Checks if spring is compressed, if so calc a force proportional do compression displacement
		real f = e->originalLength * e->originalLength; 
		f = real( f/(f+dSquared) - .5 );
#endif      
		//e->displacement = f;
		// Move particles relative to mass
		real tot = p1->massInverse + p2->massInverse; 
		if (tot <= 0) continue;
		real g = f*p1->massInverse/tot; 
		p1->pos -= p1p2 * g;
		
		g = f*p2->massInverse/tot; 
		p2->pos += p1p2 * g;
		}

	}
}

void World::fineTest(SpringBody *bodyA, SpringBody *bodyB)
{
	if( detectCollision2(bodyA, bodyB) )
	{
		bodyA->isColliding = true;
		bodyB->isColliding = true;

		Vec2f v1 = bodyA->centroid - bodyA->oldCentroid;					
		Vec2f v2 = bodyB->centroid - bodyB->oldCentroid;

		real velDiff = length_2(v1 - v2);
		if( velDiff > 0.002 && velDiff < 0.003 )
		{
			hardCollisions.push_back(1); // sound soft
		}else if( velDiff > 0.003 ){
			hardCollisions.push_back(2); // sound hard
		}

		processCollision();
	}
}

void World::testCollision()
{
	for(size_t i=0; i<bodyList.size(); i++)
	{
		for(size_t j=i+1; j<bodyList.size(); j++)
		{
			if( bodyList[i] != bodyList[j] )
			{
				fineTest( bodyList[i], bodyList[j] );
			}
		}
	}
}

real _intervalDistance(real minA, real maxA, real minB, real maxB)
{
	if(minA < minB) return minB-maxA;
	else return minA - maxB;
}

// Based on code by Benedict Bitterli
// see is gamedev.net article on Verlet Physics
// http://www.gamedev.net/page/reference/index.html/_//feature/fprogramming/a-verlet-based-approach-for-2d-game-physics-r2714
bool World::detectCollision2( SpringBody *body1, SpringBody *body2 )
{
	assert(body1 != body2 );
	if( body1->isFrozen && body2->isFrozen ) return false;
	if( !body1->bodiesOverlap(body2) ) return false;
	collisionInfo.depth = 0.0;
	collisionInfo.edge = 0x0;
	collisionInfo.normal.is(0,0);
	collisionInfo.particle = 0x0;

	real minDistance = 10000.0; // collision vector length
	real distance = 0.0;

	// just a fancy way of iterating through all edges of both bodies at once
	size_t totalEdges = body1->edges.size() + body2->edges.size();
	assert( totalEdges > 1 );
	for(size_t i=0; i<totalEdges; i++)
	{
		Edge *edge = 0x0;

		// Select correct edge according to current index
		if( i < body1->edges.size() )
		{
			edge = body1->edges[i];
		}else{
			edge = body2->edges[i - body1->edges.size()];
		}

		if( !edge->isBoundary){ continue; }

		// Calculate the axis perpendicular to this edge, simple 90 degree rotation
		
		real edge_x = edge->particles[1]->pos.x() - edge->particles[0]->pos.x();
		real edge_y = edge->particles[1]->pos.y() - edge->particles[0]->pos.y();
		Vec2f axis(-edge_y, edge_x);

		if( length_2(axis) == 0.0 ) return false; // if an edge has zero length, it is compressed into nothing, or badly set up.
		
		axis = normalize(axis);

		real minA, minB, maxA, maxB;
		projectToAxis(body1, axis, minA, maxA);
		projectToAxis(body2, axis, minB, maxB);

		distance = _intervalDistance( minA, maxA, minB, maxB );

		// If the intervals don't overlap, then no collision
		if( distance > 0.0 ){
			return false;
		}
		else if( fabs(distance) < minDistance )
		{
			minDistance = fabs(distance);
			collisionInfo.normal = axis;
			collisionInfo.edge = edge;
		}

	}

	if( minDistance == 10000.0 ) return false;


	collisionInfo.depth = minDistance;

	// Ensure that the body containing the collision edge lies in B2
	// and the one containing the colliding particle lies in B1
	if( collisionInfo.edge->parent != body2)
	{
		SpringBody *temp = body2;
		body2 = body1;
		body1 = temp;
	}

	// This is needed to make sure that the collision normal is pointing at B1
	Vec2f difference = body1->centroid - body2->centroid;
	int sign = SGN( dot(collisionInfo.normal, difference) );


	// Negate normal if it points away from b1
	if( sign != 1 ) collisionInfo.normal *= -1;

	real smallestD = 10000.0; 
	for(size_t i=0; i<body1->particles.size(); i++)
	{
		Particle *p = body1->particles[i];
		Vec2f diff = p->pos - body2->centroid;
		real distance = collisionInfo.normal.dot( diff );

		if( distance < smallestD )
		{
			smallestD = distance;
			collisionInfo.particle = p;
		}
	}

	assert( collisionInfo.particle != 0x0 );

	return true;
}

void World::processCollision()
{
	collisionInfo.normal = normalize( collisionInfo.normal );
	// Scale normal by depth
	Vec2f collisionVector = collisionInfo.normal * collisionInfo.depth;

	// Find collision edge
	Particle *p1 = collisionInfo.edge->particles[0];
	Particle *p2 = collisionInfo.edge->particles[1];

	real t = 0;

	real dx = fabs( p1->pos.x() - p2->pos.x() );
	real dy = fabs( p1->pos.y() - p2->pos.y() );
	if( dx > dy )
	{
		t = ( collisionInfo.particle->pos.x() - collisionVector.x() - p1->pos.x() ) / (p2->pos.x() - p1->pos.x() );
	}else
	{
		t = ( collisionInfo.particle->pos.y() - collisionVector.y() - p1->pos.y() ) / (p2->pos.y() - p1->pos.y() );
	}
	real lambda = real( 1.0 / ( t*t + (1.0-t) * (1.0-t) ) );

	//real e1Mass = p1->massInverse;
	//real e2Mass = p2->massInverse;
	//real collisionVertMass = collisionInfo.particle->massInverse;

	//// New, calc mass at intersection point
	//real EdgeMass = t*e2Mass + ( 1 - t )*e1Mass; 
	////Calculate the mass at the intersection point	
	//real InvCollisionMass = 1.0f/( EdgeMass + collisionVertMass );	
	//real Ratio1 = collisionVertMass*InvCollisionMass;	
	//real Ratio2 =              EdgeMass*InvCollisionMass;	

	//if(p1->massInverse > 0.0 ) p1->pos -= collisionVector * (real) ( (1.0 - t) * Ratio1 * lambda );	
	//if(p2->massInverse > 0.0 ) p2->pos -= collisionVector * (real) ( (      t) * Ratio1 * lambda );	
	//if(collisionInfo.particle->massInverse > 0.0 ) collisionInfo.particle->pos += collisionVector * Ratio2;

	// Move endpoints of collided edge
	if(p1->massInverse > 0.0 ) p1->pos -= collisionVector * (real) ( (1.0 - t) * 0.5 * lambda ); // use ratio instead of 0.5
	if(p2->massInverse > 0.0 ) p2->pos -= collisionVector * (real) ( (      t) * 0.5 * lambda );

	// No need to move every particle in a body upon collision
	// just move the colliding particle by half the coll. vector
	// let the springs to the rest
	if(collisionInfo.particle->massInverse > 0.0 ) collisionInfo.particle->pos += collisionVector * 0.5;
}


void World::removeBody( SpringBody *body )
{
	std::vector<SpringBody*>::iterator foundIterator = std::find( bodyList.begin(), bodyList.end(),  body );
	bool foundIt = false;
	if( foundIterator == bodyList.end()  )
	{
		return;
	}else
	{
		foundIt = true;
	}
	assert(body);

	while( !body->particles.empty() )
	{
		Particle *p = body->particles.back();
		body->particles.pop_back();

		particlesList.erase(
			std::remove(particlesList.begin(), particlesList.end(), p), particlesList.end()
			);
		delete p;
		
	}

	while( !body->edges.empty() )
	{
		Edge *e = body->edges.back();
		body->edges.pop_back();
		edgeList.erase(
			std::remove(edgeList.begin(), edgeList.end(), e), edgeList.end()
			);
		delete e;
	}

	bodyList.erase(std::remove(bodyList.begin(), bodyList.end(), body), bodyList.end());
	
	myGrid->removeBox( body->getBounds() );

	delete body;
	body = 0x0;
}

bool World::isSpaceEmpty( BBox* bounds )
{
	return isSpaceEmpty( bounds->xmin, bounds->ymin, bounds->xmax, bounds->ymax );
}

bool World::isSpaceEmpty( real xmin, real ymin, real xmax, real ymax )
{
	for(size_t i=0; i<bodyList.size(); i++)
	{
		{
			bodyList[i]->calcCenter();
			//if( bounds.bodiesOverlap( bodyList[i] ) );
			{
				real xmin2 = bodyList[i]->bounds.xmin;
				real ymin2 = bodyList[i]->bounds.ymin;
				real xmax2 = bodyList[i]->bounds.xmax;
				real ymax2 = bodyList[i]->bounds.ymax;	
				bool intersects = ! (( xmin > (xmax2) ) || ( (xmax) < (xmin2) ) || 
					( (ymin) > (ymax2) ) || ( (ymax) < (ymin2) ) );

				if( intersects ) return false;
			}
		}
	}
	return true;
}

World::World()
{
	worldBounds.xmin = -20;
	worldBounds.ymin = -20;

	worldBounds.xmax = 20;
	worldBounds.ymax = 20;
	myGrid = new Grid( 1000, worldBounds );

	useBruteforce = false;
}

SpringBody* World::createBody()
{
	SpringBody *b = new SpringBody();
	b->isAlive = true;
	bodyList.push_back(b);
	myGrid->addBox( b->getBounds() );
	b->calcBoundryEdges();
	return b;
}

Particle* World::createParticle( real x, real y )
{
	Particle *p = new Particle(x,y);
	particlesList.push_back(p);
	return p;
}

Edge* World::createEdge( Particle *p1, Particle *p2 )
{
	Edge *e = new Edge(p1,p2);
	edgeList.push_back(e);
	return e;
}



// 2D projection is the dot product of the projection axis and the point we want to project
void projectToAxis( SpringBody *body, const Vec2f &axis, real &min, real &max )
{
	real dpr;


	bool setFirst = false;
	for(size_t i=0; i<body->particles.size(); i++)
	{
		Particle *p = body->particles[i];
		if( !setFirst )
		{
			setFirst = true;
			dpr = dot( axis, body->particles[0]->pos );
			min = dpr;
			max = dpr;
		}else
		{
			dpr = dot( axis, p->pos );
			min = std::min<real>( dpr, min );
			max = std::max<real>( dpr, max );
		}

	}
}

